From 5aa8d6c1d63da70679dfbd131b7a87f18bcc4487 Mon Sep 17 00:00:00 2001 From: parkrrrr Date: Tue, 28 Mar 2006 22:13:53 +0000 Subject: [PATCH] Added interpolate filter --- gpsbabel/Makefile | 22 +++--- gpsbabel/filter_vecs.c | 6 ++ gpsbabel/grtcirc.c | 80 ++++++++++++++++++++ gpsbabel/grtcirc.h | 6 ++ gpsbabel/interpolate.c | 164 +++++++++++++++++++++++++++++++++++++++++ gpsbabel/readme.xml | 21 ++++++ gpsbabel/route.c | 37 ++++------ gpsbabel/testo | 11 ++- 8 files changed, 313 insertions(+), 34 deletions(-) create mode 100644 gpsbabel/interpolate.c diff --git a/gpsbabel/Makefile b/gpsbabel/Makefile index 705ad3cf3..a9677d2f3 100644 --- a/gpsbabel/Makefile +++ b/gpsbabel/Makefile @@ -51,7 +51,7 @@ FMTS=magproto.o gpx.o geo.o mapsend.o mapsource.o garmin_tables.o \ FILTERS=position.o duplicate.o arcdist.o polygon.o smplrout.o \ reverse_route.o sort.o stackfilter.o trackfilter.o discard.o \ - nukedata.o + nukedata.o interpolate.o OSJEEPS=jeeps/gpslibusb.o JEEPS=jeeps/gpsapp.o jeeps/gpscom.o \ @@ -182,7 +182,7 @@ msvc-build: echo $(OBJS) > objs.lst LINK.EXE /NOLOGO @objs.lst ./msvc/expat/libexpat.lib /out:gpsbabel.exe -# Machine generated from here down. +# Machine generated from here down. an1.o: an1.c defs.h queue.h gbtypes.h cet.h cet_util.h an1sym.h arcdist.o: arcdist.c defs.h queue.h gbtypes.h cet.h cet_util.h \ filterdefs.h grtcirc.h @@ -195,8 +195,6 @@ brauniger_iq.o: brauniger_iq.c defs.h queue.h gbtypes.h cet.h cet_util.h \ jeeps/gpsmath.h jeeps/gpsmem.h jeeps/gpsrqst.h jeeps/gpsinput.h \ jeeps/gpsproj.h cet.o: cet.c defs.h queue.h gbtypes.h cet.h cet_util.h -cetus.o: cetus.c defs.h queue.h gbtypes.h cet.h cet_util.h \ - coldsync/palm.h coldsync/../gbtypes.h coldsync/pdb.h cet_util.o: cet_util.c defs.h queue.h gbtypes.h cet.h cet_util.h \ cet/iso_8859_1.h cet/iso_8859_15.h cet/ansi_x3_4_1968.h cet/cp1252.h \ cet/atarist.h cet/baltic.h cet/bs_4730.h cet/bs_viewdata.h cet/cp1250.h \ @@ -224,6 +222,8 @@ cet_util.o: cet_util.c defs.h queue.h gbtypes.h cet.h cet_util.h \ cet/nextstep.h cet/nf_z_62_010.h cet/nf_z_62_010__1973_.h \ cet/ns_4551_1.h cet/ns_4551_2.h cet/pt.h cet/pt2.h cet/sami.h \ cet/sen_850200_b.h cet/sen_850200_c.h cet/tcvn.h cet/viscii.h cet/vps.h +cetus.o: cetus.c defs.h queue.h gbtypes.h cet.h cet_util.h \ + coldsync/palm.h coldsync/../gbtypes.h coldsync/pdb.h coastexp.o: coastexp.c defs.h queue.h gbtypes.h cet.h cet_util.h \ xmlgeneric.h uuid.h compegps.o: compegps.c defs.h queue.h gbtypes.h cet.h cet_util.h \ @@ -299,18 +299,20 @@ ignrando.o: ignrando.c defs.h queue.h gbtypes.h cet.h cet_util.h \ inifile.o: inifile.c defs.h queue.h gbtypes.h cet.h cet_util.h inifile.h internal_styles.o: internal_styles.c defs.h queue.h gbtypes.h cet.h \ cet_util.h +interpolate.o: interpolate.c defs.h queue.h gbtypes.h cet.h cet_util.h \ + filterdefs.h grtcirc.h kml.o: kml.c defs.h queue.h gbtypes.h cet.h cet_util.h xmlgeneric.h lowranceusr.o: lowranceusr.c defs.h queue.h gbtypes.h cet.h cet_util.h -maggeo.o: maggeo.c defs.h queue.h gbtypes.h cet.h cet_util.h xmlgeneric.h \ - magellan.h -magnav.o: magnav.c defs.h queue.h gbtypes.h cet.h cet_util.h \ - coldsync/palm.h coldsync/../gbtypes.h coldsync/pdb.h mag_pdb.o: mag_pdb.c defs.h queue.h gbtypes.h cet.h cet_util.h \ coldsync/palm.h coldsync/../gbtypes.h coldsync/pdb.h jeeps/gpsmath.h \ jeeps/gps.h jeeps/../defs.h jeeps/gpsport.h jeeps/gpsserial.h \ jeeps/gpssend.h jeeps/gpsread.h jeeps/gpsutil.h jeeps/gpsapp.h \ jeeps/gpsprot.h jeeps/gpscom.h jeeps/gpsfmt.h jeeps/gpsmath.h \ jeeps/gpsmem.h jeeps/gpsrqst.h jeeps/gpsinput.h jeeps/gpsproj.h +maggeo.o: maggeo.c defs.h queue.h gbtypes.h cet.h cet_util.h xmlgeneric.h \ + magellan.h +magnav.o: magnav.c defs.h queue.h gbtypes.h cet.h cet_util.h \ + coldsync/palm.h coldsync/../gbtypes.h coldsync/pdb.h magproto.o: magproto.c defs.h queue.h gbtypes.h cet.h cet_util.h \ magellan.h main.o: main.c defs.h queue.h gbtypes.h cet.h cet_util.h filterdefs.h @@ -390,8 +392,8 @@ tpo.o: tpo.c defs.h queue.h gbtypes.h cet.h cet_util.h jeeps/gpsmath.h \ trackfilter.o: trackfilter.c defs.h queue.h gbtypes.h cet.h cet_util.h \ filterdefs.h strptime.h unicsv.o: unicsv.c defs.h queue.h gbtypes.h cet.h cet_util.h csv_util.h -util_crc.o: util_crc.c util.o: util.c defs.h queue.h gbtypes.h cet.h cet_util.h +util_crc.o: util_crc.c uuid.o: uuid.c uuid.h vcf.o: vcf.c defs.h queue.h gbtypes.h cet.h cet_util.h jeeps/gpsmath.h \ jeeps/gps.h jeeps/../defs.h jeeps/gpsport.h jeeps/gpsserial.h \ @@ -494,5 +496,5 @@ jeeps/gpsutil.o: jeeps/gpsutil.c jeeps/gps.h jeeps/../defs.h \ jeeps/gpsinput.h jeeps/gpsproj.h shapelib/dbfopen.o: shapelib/dbfopen.c shapelib/shapefil.h shapelib/shpopen.o: shapelib/shpopen.c shapelib/shapefil.h -internal_styles.c: mkstyle.sh style/arc.style style/cambridge.style style/csv.style style/cup.style style/custom.style style/dna.style style/fugawi.style style/garmin301.style style/garmin_poi.style style/geonet.style style/gpsdrive.style style/gpsdrivetrack.style style/gpsman.style style/mapconverter.style style/mxf.style style/nima.style style/openoffice.style style/README.style style/s_and_t.style style/saplus.style style/tabsep.style style/xmap.style style/xmapwpt.style +internal_styles.c: mkstyle.sh style/README.style style/arc.style style/cambridge.style style/csv.style style/cup.style style/custom.style style/dna.style style/fugawi.style style/garmin301.style style/garmin_poi.style style/geonet.style style/gpsdrive.style style/gpsdrivetrack.style style/gpsman.style style/mapconverter.style style/mxf.style style/nima.style style/openoffice.style style/s_and_t.style style/saplus.style style/tabsep.style style/xmap.style style/xmapwpt.style ./mkstyle.sh > internal_styles.c || (rm -f internal_styles.c ; exit 1) diff --git a/gpsbabel/filter_vecs.c b/gpsbabel/filter_vecs.c index 3b4a5feb8..b724da9e4 100644 --- a/gpsbabel/filter_vecs.c +++ b/gpsbabel/filter_vecs.c @@ -40,6 +40,7 @@ extern filter_vecs_t stackfilt_vecs; extern filter_vecs_t trackfilter_vecs; extern filter_vecs_t discard_vecs; extern filter_vecs_t nuke_vecs; +extern filter_vecs_t interpolatefilt_vecs; static fl_vecs_t filter_vec_list[] = { @@ -103,6 +104,11 @@ fl_vecs_t filter_vec_list[] = { "nuketypes", "Remove all waypoints, tracks, or routes" }, + { + &interpolatefilt_vecs, + "interpolate", + "Interpolate between trackpoints" + }, { NULL, NULL, diff --git a/gpsbabel/grtcirc.c b/gpsbabel/grtcirc.c index d5e5cddbb..3676eeaa5 100644 --- a/gpsbabel/grtcirc.c +++ b/gpsbabel/grtcirc.c @@ -203,3 +203,83 @@ double linedist(double lat1, double lon1, return 0; } +/* + * Compute the position of a point partially along the geodesic from + * lat1,lon1 to lat2,lon2 + * + * Ref: http://mathworld.wolfram.com/RotationFormula.html + */ + +void linepart(double lat1, double lon1, + double lat2, double lon2, + double frac, + double *reslat, double *reslon ) { + + double x1,y1,z1; + double x2,y2,z2; + double xa,ya,za,la; + double xr, yr, zr; + double xx, yx, zx; + + double theta = 0; + double phi = 0; + double cosphi = 0; + double sinphi = 0; + + /* degrees to radians */ + lat1 *= M_PI/180.0; lon1 *= M_PI/180.0; + lat2 *= M_PI/180.0; lon2 *= M_PI/180.0; + + /* polar to ECEF rectangular */ + x1 = cos(lon1)*cos(lat1); y1 = sin(lat1); z1 = sin(lon1)*cos(lat1); + x2 = cos(lon2)*cos(lat2); y2 = sin(lat2); z2 = sin(lon2)*cos(lat2); + + /* 'a' is the axis; the line that passes through the center of the earth + * and is perpendicular to the great circle through point 1 and point 2 + * It is computed by taking the cross product of the '1' and '2' vectors.*/ + crossproduct( x1, y1, z1, x2, y2, z2, &xa, &ya, &za ); + la = sqrt(xa*xa+ya*ya+za*za); + + if ( la ) { + xa /= la; + ya /= la; + za /= la; + } + *reslat = lat1; + *reslon = lon1; + /* if la is zero, the points are either equal or directly opposite + * each other. Either way, there's no single geodesic, so we punt. */ + if ( la ) { + crossproduct( x1, y1, z1, xa, ya, za, &xx, &yx, &zx ); + + + theta = atan2( dotproduct(xx,yx,zx,x2,y2,z2), + dotproduct(x1,y1,z1,x2,y2,z2)); + + phi = frac * theta; + cosphi = cos(phi); + sinphi = sin(phi); + + + /* The second term of the formula from the mathworld reference is always + * zero, because r (lat1,lon1) is always perpendicular to n (a here) */ + xr = x1*cosphi + xx * sinphi; + yr = y1*cosphi + yx * sinphi; + zr = z1*cosphi + zx * sinphi; + + if ( xr > 1 ) xr = 1; + if ( xr < -1 ) xr = -1; + if ( yr > 1 ) yr = 1; + if ( yr < -1 ) yr = -1; + if ( zr > 1 ) zr = 1; + if ( zr < -1 ) zr = -1; + + *reslat = asin(yr) * 180 / M_PI; + if( xr == 0 && zr == 0 ) { + *reslon = 0; + } + else { + *reslon = atan2( zr, xr ) * 180 / M_PI; + } + } +} diff --git a/gpsbabel/grtcirc.h b/gpsbabel/grtcirc.h index de84aea4c..250e0d2ab 100644 --- a/gpsbabel/grtcirc.h +++ b/gpsbabel/grtcirc.h @@ -25,3 +25,9 @@ double linedist(double lat1, double lon1, double lat3, double lon3 ); double tomiles( double rads ); + +void linepart(double lat1, double lon1, + double lat2, double lon2, + double frac, + double *reslat, double *reslon ); + diff --git a/gpsbabel/interpolate.c b/gpsbabel/interpolate.c new file mode 100644 index 000000000..f9cbdb2d7 --- /dev/null +++ b/gpsbabel/interpolate.c @@ -0,0 +1,164 @@ +/* + Interpolate filter + + Copyright (C) 2002 Robert Lipe, robertlipe@usa.net + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA + + */ + +#include "defs.h" +#include "filterdefs.h" +#include "grtcirc.h" + +#define MYNAME "Interpolate filter" + +static char *opt_interval = NULL; +int interval = 0; +static char *opt_dist = NULL; +double dist = 0; + +static +arglist_t interpfilt_args[] = { + {"time", &opt_interval, "Time interval in seconds", NULL, + ARGTYPE_BEGIN_EXCL | ARGTYPE_BEGIN_REQ | ARGTYPE_INT, + "0", NULL }, + {"distance", &opt_dist, "Distance interval in miles or kilometers", + NULL, ARGTYPE_END_EXCL | ARGTYPE_END_REQ | ARGTYPE_STRING, + ARG_NOMINMAX }, + ARG_TERMINATOR +}; + +void +interpfilt_process(void) +{ + queue *backuproute = NULL; + queue *elem, *tmp, *elem2, *tmp2; + route_head *rte_new; + int count = 0; + int first = 0; + double lat1, lon1; + int time1; + int timen; + double distn; + double curdist; + double rt1, rn1, rt2, rn2; + + track_backup( &count, &backuproute ); + route_flush_all_tracks(); + QUEUE_FOR_EACH( backuproute, elem, tmp ) + { + route_head *rte_old = (route_head *)elem; + + rte_new = route_head_alloc(); + rte_new->rte_name = xstrdup( rte_old->rte_name ); + rte_new->rte_desc = xstrdup( rte_old->rte_desc ); + rte_new->fs = fs_chain_copy( rte_old->fs ); + rte_new->rte_num = rte_old->rte_num; + track_add_head( rte_new ); + first = 1; + QUEUE_FOR_EACH( &rte_old->waypoint_list, elem2, tmp2 ) + { + waypoint *wpt = (waypoint *)elem2; + if ( first ) { + first = 0; + } + else { + if ( opt_interval && + wpt->creation_time - time1 > interval ) { + for ( timen = time1+interval; + timen < wpt->creation_time; + timen += interval ) { + waypoint *wpt_new = waypt_dupe(wpt); + wpt_new->creation_time = timen; + xfree(wpt_new->shortname); + xfree(wpt_new->description); + wpt_new->shortname = wpt_new->description = 0; + linepart( lat1, lon1, + wpt->latitude, wpt->longitude, + (double)(timen-time1)/ + (double)(wpt->creation_time-time1), + &wpt_new->latitude, + &wpt_new->longitude ); + track_add_wpt( rte_new, wpt_new); + } + } + else if ( opt_dist ) { + rt1 = lat1 * M_PI / 180; + rn1 = lon1 * M_PI / 180; + rt2 = wpt->latitude * M_PI / 180; + rn2 = wpt->longitude * M_PI / 180; + curdist = gcdist( rt1, rn1, rt2, rn2 ); + curdist = tomiles(curdist); + if ( curdist > dist ) { + for ( distn = dist; + distn < curdist; + distn += dist ) { + waypoint *wpt_new = waypt_dupe(wpt); + wpt_new->creation_time = distn/curdist* + (wpt->creation_time - time1) + time1; + xfree(wpt_new->shortname); + xfree(wpt_new->description); + wpt_new->shortname = wpt_new->description = 0; + linepart( lat1, lon1, + wpt->latitude, wpt->longitude, + distn/curdist, + &wpt_new->latitude, + &wpt_new->longitude ); + track_add_wpt( rte_new, wpt_new); + } + } + } + } + track_add_wpt( rte_new, waypt_dupe(wpt)); + + lat1 = wpt->latitude; + lon1 = wpt->longitude; + time1 = wpt->creation_time; + } + } + route_flush( backuproute ); + xfree( backuproute ); +} + +void +interpfilt_init(const char *args) { + + char *fm; + if ( opt_interval && opt_dist ) { + fatal( MYNAME ": Can't interpolate on both time and distance.\n"); + } + else if ( opt_interval ) { + interval = atoi(opt_interval); + } + else if ( opt_dist ) { + dist = strtod(opt_dist, &fm); + if ((*fm == 'k') || (*fm == 'K')) { + /* distance is kilometers, convert to miles */ + dist *= .6214; + } + } + else { + fatal( MYNAME ": No interval specified.\n"); + } +} + +filter_vecs_t interpolatefilt_vecs = { + interpfilt_init, + interpfilt_process, + NULL, + NULL, + interpfilt_args +}; diff --git a/gpsbabel/readme.xml b/gpsbabel/readme.xml index 4cbb969cd..5c85ac399 100644 --- a/gpsbabel/readme.xml +++ b/gpsbabel/readme.xml @@ -2159,5 +2159,26 @@ gpsbabel -t \ gpsbabel -i gpx -f bigfile.gpx -x nuketypes,waypoints,routes -o gpx -F tracksonly.gpx +
+ INTERPOLATE + + This filter modifies any tracks so that either the distance or the time + between consecutive points is no less than the specified interval. Where + points are missing, the filter fills them in by following a straight + line (actually a great circle) between the adjacent points. You + must specify either the "distance" or the "time" option. + + gpsbabel -i gpx -f track.gpx -x interpolate,time=10 -o gpx -f newtrack.gpx + Reads track.gpx and inserts points wherever two adjacent + trackpoints are more than 10 seconds apart + gpsbabel -i gpx -f track.gpx -x interpolate,distance=15k -o gpx -f newtrack.gpx + Reads track.gpx and inserts points wherever two adjacent + trackpoints are more than 15 kilometers apart + gpsbabel -i gpx -f track.gpx -x interpolate,distance=2m -o gpx -f newtrack.gpx + Reads track.gpx and inserts points wherever two adjacent + trackpoints are more than 2 miles apart +
+ + diff --git a/gpsbabel/route.c b/gpsbabel/route.c index 9c75289db..6238225c9 100644 --- a/gpsbabel/route.c +++ b/gpsbabel/route.c @@ -102,23 +102,6 @@ track_del_head(route_head *rte) trk_head_ct--; } -void -route_debug_count( char *str, queue *q ) -{ - queue *elem, *tmp; - int count = 0; - QUEUE_FOR_EACH( q, elem, tmp ) { - count++; - } - fprintf( stderr, "%s %d\n", str, count ); -} - -void -route_debug_count_main( char *str ) -{ - route_debug_count( str, &my_route_head ); -} - static route_head * common_route_by_name(queue *routes, const char *name) @@ -153,12 +136,14 @@ any_route_add_wpt(route_head *rte, waypoint *wpt, int *ct ) { ENQUEUE_TAIL(&rte->waypoint_list, &wpt->Q); rte->rte_waypt_ct++; /* waypoints in this route */ - (*ct)++; - if (wpt->shortname == NULL) { - char tmpnam[10]; - snprintf(tmpnam, sizeof(tmpnam), "RPT%03d",*ct); - wpt->shortname = xstrdup(tmpnam); - wpt->wpt_flags.shortname_is_synthetic = 1; + if ( ct ) { + (*ct)++; + if (wpt->shortname == NULL) { + char tmpnam[10]; + snprintf(tmpnam, sizeof(tmpnam), "RPT%03d",*ct); + wpt->shortname = xstrdup(tmpnam); + wpt->wpt_flags.shortname_is_synthetic = 1; + } } } @@ -168,6 +153,12 @@ route_add_wpt( route_head *rte, waypoint *wpt ) any_route_add_wpt( rte, wpt, &rte_waypts ); } +void +track_add_wpt( route_head *rte, waypoint *wpt ) +{ + any_route_add_wpt( rte, wpt, NULL ); +} + waypoint * route_find_waypt_by_name( route_head *rh, const char *name ) { diff --git a/gpsbabel/testo b/gpsbabel/testo index dd2568137..6030fa548 100755 --- a/gpsbabel/testo +++ b/gpsbabel/testo @@ -425,7 +425,7 @@ compare ${TMPDIR}/maggpx.trk ${TMPDIR}/gpxtrack.gpx rm -f ${TMPDIR}/route.mapsend ${PNAME} -r -i mapsend -f reference/route/route.mapsend -o mapsend \ -F ${TMPDIR}/route.mapsend -compare ${TMPDIR}/route.mapsend reference/route/ +bincompare ${TMPDIR}/route.mapsend reference/route/route.mapsend # # MAPSEND track format @@ -969,6 +969,15 @@ ${PNAME} -i gpx -f ${TMPDIR}/alltypes.gpx -x nuketypes,waypoints,tracks -o gpx - ${PNAME} -i gpx -f ${TMPDIR}/wpts.gpx -f ${TMPDIR}/trks.gpx -f ${TMPDIR}/rtes.gpx -o gpx -F ${TMPDIR}/merged.gpx compare ${TMPDIR}/alltypes.gpx ${TMPDIR}/merged.gpx +# +# Interpolate filter +# + +${PNAME} -i gpx -f reference/track/simpletrack.gpx -x interpolate,distance=50m -o gpx -F ${TMPDIR}/interp.gpx +compare reference/track/interptrack.gpx ${TMPDIR}/interp.gpx +${PNAME} -i gpx -f reference/track/simpletrack.gpx -x interpolate,time=1 -o gpx -F ${TMPDIR}/tinterp.gpx +compare reference/track/tinterptrack.gpx ${TMPDIR}/tinterp.gpx + # # Universal CSV - unicsv # -- 2.30.2